﻿
// 2019/10/30: 首个记录（基于GlobalObjectsSampleV1.cs 2019/4/11版本修改）
// 2020/1/14: 接口完备性
// 2021/4/6: 基于ASEvaAPI 2.0
// 2022/10/18: 增加SO标签
// 2022/11/3: 使用SetTime复制全部时间信息
// 2023/10/10: 补充部分英文注释

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;
using SO = System.Reflection.ObfuscationAttribute;

namespace GlobalObjectsSampleV2
{
    [SO]
    enum DynamicObjectType
    {
        Unknown = 0,
        Car = 1, // 轿车
        Van = 2, // 面包车
        Truck = 3, // 货车
        Trailer = 4, // 拖车
        Semitrailer = 5, // 货柜车
        Bus = 6, // 大巴
        Motorbike = 7, // 摩托车
        Bicycle = 8, // 自行车
        Train = 9, // 火车
        Tram = 10, // 有轨电车
        Pedestrian = 11, // 行人
        Wheelchair = 12, // 轮椅
        Animal = 13, // 动物
    }

    class DynamicObject
    {
        public int ID { get; set; } // ID of the object, 0 as subject, 1~ as other objects / 目标物ID，0表示为主目标物，1~表示一般目标物
        public DynamicObjectType Type { get; set; } // Classification of the object / 目标物类型
        public double CenterLongitude { get; set; } // [°E] Longitude of the object's box center / 目标物中心的经度
        public double CenterLatitude { get; set; } // [°N] Latitude of the object's box center / 目标物中心的纬度，北为正方向
        public double? CenterAltitude { get; set; } // [m] Altitude of the object's box center / 目标物中心的海拔
        public double? Orientation { get; set; } // [deg] Heading of the object's box, 0 as north, CCW is positive / 目标物朝向，北为0，逆时针为正方向
        public double? Width { get; set; } // [m] Width of the object's box / 目标物宽度
        public double? Length { get; set; } // [m] Length of the object's box / 目标物长度
        public double? Height { get; set; } // [m] Height of the object's box / 目标物高度
        public double? Speed { get; set; } // [KPH] Speed of the object's box along the heading / 目标物速度
        public double? AccelX { get; set; } // [m/s²] Acceleration of the object's box along the heading / 目标物纵向加速度
        public double? Curvature { get; set; } // [1/m] Turn curvature of the object's rear axis center / 目标物转弯曲率
        public double? YawRate { get; set; } // [deg/s] Yaw rate of the object's box / 目标物横摆角速度
        public double? Pitch { get; set; } // [deg] Pitch angle of the object's box, heading down is positive / 目标物俯仰角，车头朝下为正
        public double? Roll { get; set; } // [deg] Roll angle of the object's box, right side heading down is positive / 目标物横滚角，右侧朝下为正
        public double? RearAxisLongitudinalOffset { get; set; } // [m] Longitudinal distance between the object's box center and rear axis center, always positive / 目标物自身坐标系下，后轴中心与物体中心的纵向距离，恒为正
        public double? RearAxisHeightOffset { get; set; } // [m] Height difference of the object's box center and rear axis center, always positive / 目标物自身坐标系下，后轴中心与物体中心的高度差，恒为正
        public ColorRGBA? Color { get; set; } // Color of the object / 目标物颜色
    }

    class GlobalObjectsSample : Sample
    {
        public List<DynamicObject> Objects { get; set; } // List of objects / 目标物列表

        public GlobalObjectsSample()
        {
            Objects = new List<DynamicObject>();
        }

        public static string Title
        {
            get
            {
                return "Object count,First object's ID,First object's type,First object's center longitude[°E],First object's center latitude[°N],First object's ground altitude[m],First object's orientation[°],First object's width[m],First object's length[m],First object's height[m],First object's speed[KPH],First object's longitudinal acceleration[m/s²],First object's curvature[1/m],First object's yaw rate[°/s],First object's pitch angle[°],First object's roll angle[°],First object's rear-axis longitudinal offset[m],First object's rear-axis height offset[m],First object's color(R-component),First object's color(G-component),First object's color(B-component),First object's reserved1,First object's reserved2,First object's reserved3,First object's reserved4,First object's reserved5,First object's reserved6";
            }
        }

        public static string Protocol
        {
            get
            {
                return "global-objects-sample-v2";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new String[] { "global-objects-sample-v1", "global-objects-sample-v2" };
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.SetTime(this);
            sample.Protocol = Protocol;

            var data = new GeneralSampleValue[1 + 26 * Objects.Count];
            sample.NumberOfSignificants = data.Length;

            data[0] = new GeneralSampleValue((double)Objects.Count);

            int baseIndex = 1;
            for (int i = 0; i < Objects.Count; i++)
            {
                var obj = Objects[i];
                data[baseIndex + 0] = new GeneralSampleValue((double)obj.ID);
                data[baseIndex + 1] = new GeneralSampleValue((double)(uint)obj.Type);
                data[baseIndex + 2] = new GeneralSampleValue(obj.CenterLongitude);
                data[baseIndex + 3] = new GeneralSampleValue(obj.CenterLatitude);
                if (obj.CenterAltitude != null) data[baseIndex + 4] = new GeneralSampleValue(obj.CenterAltitude.Value);
                if (obj.Orientation != null) data[baseIndex + 5] = new GeneralSampleValue(obj.Orientation.Value);
                if (obj.Width != null) data[baseIndex + 6] = new GeneralSampleValue(obj.Width.Value);
                if (obj.Length != null) data[baseIndex + 7] = new GeneralSampleValue(obj.Length.Value);
                if (obj.Height != null) data[baseIndex + 8] = new GeneralSampleValue(obj.Height.Value);
                if (obj.Speed != null) data[baseIndex + 9] = new GeneralSampleValue(obj.Speed.Value);
                if (obj.AccelX != null) data[baseIndex + 10] = new GeneralSampleValue(obj.AccelX.Value);
                if (obj.Curvature != null) data[baseIndex + 11] = new GeneralSampleValue(obj.Curvature.Value);
                if (obj.YawRate != null) data[baseIndex + 12] = new GeneralSampleValue(obj.YawRate.Value);
                if (obj.Pitch != null) data[baseIndex + 13] = new GeneralSampleValue(obj.Pitch.Value);
                if (obj.Roll != null) data[baseIndex + 14] = new GeneralSampleValue(obj.Roll.Value);
                if (obj.RearAxisLongitudinalOffset != null) data[baseIndex + 15] = new GeneralSampleValue(obj.RearAxisLongitudinalOffset.Value);
                if (obj.RearAxisHeightOffset != null) data[baseIndex + 16] = new GeneralSampleValue(obj.RearAxisHeightOffset.Value);
                if (obj.Color != null)
                {
                    data[baseIndex + 17] = new GeneralSampleValue(obj.Color.Value.R);
                    data[baseIndex + 18] = new GeneralSampleValue(obj.Color.Value.G);
                    data[baseIndex + 19] = new GeneralSampleValue(obj.Color.Value.B);
                }

                baseIndex += 26;
            }

            sample.Values = data.ToList();

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            SetTime(sample);

            #region global-objects-sample-v2
            if (sample.Protocol == "global-objects-sample-v2")
            {
                if (sample.Values.Count < 1) return false;
                if (!sample.Values[0].IsNumber()) return false;

                int objectCount = sample.Values[0].ToInt().Value;
                if (sample.Values.Count != 1 + 26 * objectCount) return false;

                Objects = new List<DynamicObject>();
                int baseIndex = 1;
                for (int i = 0; i < objectCount; i++)
                {
                    var obj = new DynamicObject();
                    obj.ID = sample.Values[baseIndex + 0].ToInt().Value;
                    obj.Type = (DynamicObjectType)sample.Values[baseIndex + 1].ToInt().Value;
                    obj.CenterLongitude = sample.Values[baseIndex + 2].ToDouble().Value;
                    obj.CenterLatitude = sample.Values[baseIndex + 3].ToDouble().Value;
                    obj.CenterAltitude = sample.Values[baseIndex + 4].ToDouble();
                    obj.Orientation = sample.Values[baseIndex + 5].ToDouble();
                    obj.Width = sample.Values[baseIndex + 6].ToDouble();
                    obj.Length = sample.Values[baseIndex + 7].ToDouble();
                    obj.Height = sample.Values[baseIndex + 8].ToDouble();
                    obj.Speed = sample.Values[baseIndex + 9].ToDouble();
                    obj.AccelX = sample.Values[baseIndex + 10].ToDouble();
                    obj.Curvature = sample.Values[baseIndex + 11].ToDouble();
                    obj.YawRate = sample.Values[baseIndex + 12].ToDouble();
                    obj.Pitch = sample.Values[baseIndex + 13].ToDouble();
                    obj.Roll = sample.Values[baseIndex + 14].ToDouble();
                    obj.RearAxisLongitudinalOffset = sample.Values[baseIndex + 15].ToDouble();
                    obj.RearAxisHeightOffset = sample.Values[baseIndex + 16].ToDouble();
                    if (sample.Values[baseIndex + 17].IsNumber() && sample.Values[baseIndex + 18].IsNumber() && sample.Values[baseIndex + 19].IsNumber())
                    {
                        obj.Color = new ColorRGBA((byte)sample.Values[baseIndex + 17].ToInt().Value, (byte)sample.Values[baseIndex + 18].ToInt().Value, (byte)sample.Values[baseIndex + 19].ToInt().Value);
                    }
                    Objects.Add(obj);

                    baseIndex += 26;
                }

                return true;
            }
            #endregion

            #region global-objects-sample-v1
            else if (sample.Protocol == "global-objects-sample-v1")
            {
                if (sample.Values.Count < 1) return false;
                if (!sample.Values[0].IsNumber()) return false;

                int objectCount = sample.Values[0].ToInt().Value;
                if (sample.Values.Count != 1 + 10 * objectCount) return false;

                Objects = new List<DynamicObject>();
                int baseIndex = 1;
                for (int i = 0; i < objectCount; i++)
                {
                    var obj = new DynamicObject();
                    obj.ID = sample.Values[baseIndex + 0].ToInt().Value;
                    obj.Type = (DynamicObjectType)sample.Values[baseIndex + 1].ToInt().Value;
                    obj.CenterLongitude = sample.Values[baseIndex + 2].ToDouble().Value;
                    obj.CenterLatitude = sample.Values[baseIndex + 3].ToDouble().Value;
                    obj.CenterAltitude = sample.Values[baseIndex + 4].ToDouble();
                    obj.Orientation = sample.Values[baseIndex + 5].ToDouble();
                    obj.Width = sample.Values[baseIndex + 6].ToDouble();
                    obj.Length = sample.Values[baseIndex + 7].ToDouble();
                    obj.Height = sample.Values[baseIndex + 8].ToDouble();
                    obj.Speed = sample.Values[baseIndex + 9].ToDouble();
                    Objects.Add(obj);

                    baseIndex += 10;
                }

                return true;
            }
            #endregion

            return false;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as GlobalObjectsSample;
            var s2 = input.s2 as GlobalObjectsSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new GlobalObjectsSample();
            foreach (var s1Obj in s1.Objects)
            {
                foreach (var s2Obj in s2.Objects)
                {
                    if (s1Obj.ID == s2Obj.ID)
                    {
                        var obj = new DynamicObject();
                        obj.ID = s1Obj.ID;
                        obj.Type = w1 > w2 ? s1Obj.Type : s2Obj.Type;
                        obj.CenterLongitude = s1Obj.CenterLongitude * w1 + s2Obj.CenterLongitude * w2;
                        obj.CenterLatitude = s1Obj.CenterLatitude * w1 + s2Obj.CenterLatitude * w2;
                        obj.CenterAltitude = s1Obj.CenterAltitude * w1 + s2Obj.CenterAltitude * w2;
                        obj.Orientation = interpolateAngle(s1Obj.Orientation, w1, s2Obj.Orientation, w2);
                        obj.Width = s1Obj.Width * w1 + s2Obj.Width * w2;
                        obj.Length = s1Obj.Length * w1 + s2Obj.Length * w2;
                        obj.Height = s1Obj.Height * w1 + s2Obj.Height * w2;
                        obj.Speed = s1Obj.Speed * w1 + s2Obj.Speed * w2;
                        obj.AccelX = s1Obj.AccelX * w1 + s2Obj.AccelX * w2;
                        obj.Curvature = s1Obj.Curvature * w1 + s2Obj.Curvature * w2;
                        obj.YawRate = s1Obj.YawRate * w1 + s2Obj.YawRate * w2;
                        obj.Pitch = s1Obj.Pitch * w1 + s2Obj.Pitch * w2;
                        obj.Roll = s1Obj.Roll * w1 + s2Obj.Roll * w2;
                        obj.RearAxisLongitudinalOffset = s1Obj.RearAxisLongitudinalOffset * w1 + s2Obj.RearAxisLongitudinalOffset * w2;
                        obj.RearAxisHeightOffset = s1Obj.RearAxisHeightOffset * w1 + s2Obj.RearAxisHeightOffset * w2;
                        obj.Color = w1 > w2 ? s1Obj.Color : s2Obj.Color;
                        output.Objects.Add(obj);
                        break;
                    }
                }
            }

            return output;
        }

        private double? interpolateAngle(double? a1, double w1, double? a2, double w2)
        {
            if (a1 == null || a2 == null) return null;

            var deg2rad = Math.PI / 180;
            var x1 = Math.Cos(a1.Value * deg2rad);
            var y1 = Math.Sin(a1.Value * deg2rad);
            var x2 = Math.Cos(a2.Value * deg2rad);
            var y2 = Math.Sin(a2.Value * deg2rad);

            var xo = x1 * w1 + x2 * w2;
            var yo = y1 * w1 + y2 * w2;
            if (xo == 0 && yo == 0) return null;

            return Math.Atan2(yo, xo) / deg2rad;
        }
    }
}
